classdef AbstractLTIUnit < handle
    %ABSTRACTLTIUNIT Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        name
        tfFunction
        
        tc     % 1/e time of step response
        bw_3dB % -3dB bandwidth
        gain   % DC gain
        tList = []
        
        wMax
        wMin = 0.01;
        
        isStable
        isCausal
        isFiniteGain
        is1stOrderApproxAvailable
    end
    properties (Access = {?EquipmentControl.FeedBackOptimization.LTIUnit.LowPassFilter, ...
                          ?EquipmentControl.FeedBackOptimization.LTIUnit.PIDController, ...
                          ?EquipmentControl.FeedBackOptimization.LTIUnit.StaticGain})
        num
        den
    end
    
    methods
        function obj = AbstractLTIUnit(name, num, den)
            %ABSTRACTLTIUNIT Construct an instance of this class
            %   Detailed explanation goes here
            obj.name = name;
            obj.num = num;
            obj.den = den;
            obj.tfFunction = tf(num, den);
            
            obj.characterization();
        end
        
        function obj = characterization(obj)
            len = max(length(obj.num), length(obj.den));            
            newNum = padZerosLeft(obj.num, len); 
            newDen = padZerosLeft(obj.den, len);
            
            % stability
            r = roots(newDen);
            obj.isStable = max(real(r)) < 0.0;                
            
            % -3dB bandwidth
            [mag, ~, w]=bode(obj.tfFunction);
            [~, idx]=min(abs(mag(:)-sqrt(0.5)));
            try
                obj.bw_3dB=w(idx);
                obj.wMax = 10.^ceil(log10(obj.bw_3dB));
            catch
                obj.bw_3dB = nan;
            end
            
            % gain & tc
            orderNum = minmax(find(newNum ~= 0)); orderDen = minmax(find(newDen ~= 0));            
            minOrder = min(orderNum(2), orderDen(2)); 
            obj.gain = newNum(minOrder)/newDen(minOrder);
            
            obj.isFiniteGain = isfinite(obj.gain) && obj.gain > 0;
            obj.isCausal = orderNum(1) >= orderDen(1);
            
            if obj.isCausal && obj.isFiniteGain && ~isnan(obj.bw_3dB)
                [y, tGrid] = step(obj.tfFunction, obj.timeGrid);
                [~, idx] = min( abs( y-0.6321*obj.gain ) ); % 1-1/e = 0.6321
                
                obj.tList = tGrid;
                obj.tc = tGrid(idx);    
                obj.is1stOrderApproxAvailable = true;
            else
                obj.tc = 0.0;
                obj.is1stOrderApproxAvailable = false;
            end
        end
        
        function tList = timeGrid(obj, nT)
            if nargin == 1
                nT = 201;
            end
            [~, t] = step(obj.tfFunction);
            tMax = 2.0*t(end);% dt = 0.1*(t(2)-t(1));  tList = 0:dt:tMax;
            tList = linspace(0, tMax, nT);
        end
        
        function tfFunc = getTFfunction(obj)
            tfFunc = tf(obj.num, obj.den);
        end
        
        function [Gm,Pm,Wgm,Wpm] = margins(obj)
            [Gm,Pm,Wgm,Wpm] = margin(obj.tfFunction);
        end
        
        function [tf1, tau1, tDelay] = firstOrderApprox(obj)
            % use numerical results
            % test respose
            if obj.is1stOrderApproxAvailable
                [y, t] = step(obj.tfFunction, obj.tList );                
                dt = t(2) - t(1);
                [maxK, idx] = max(diff(y)); slope = maxK/dt;
                b=y(idx)-slope*t(idx); % linear regime => y=slope*t+b
                tDelay = -b/slope;
                
                [~, idx] = min(abs(y-obj.gain*(1.0-1./exp(1))));
                tau1 = t(idx) - tDelay;
                s=tf('s');
                tf1 = obj.gain/(1+tau1*s) * exp(-tDelay*s);
            else
                error('No first order appoximation.');
            end
        end
        
        function [y, t] = stepResponse(obj, varargin)            
            p=inputParser;
            axCheck = @(x) isa(x, 'matlab.graphics.axis.Axes') || isempty(x);
            p.addParameter('tList', obj.tList, @isnumeric);
            p.addParameter('isPlot', true, @islogical);
            p.addParameter('calcApprox', true, @islogical);
            p.addParameter('axes', [], axCheck);
            p.parse(varargin{:});
            
            if obj.isCausal
                [y, t] = step(obj.tfFunction, p.Results.tList);
                if obj.is1stOrderApproxAvailable && p.Results.calcApprox
                    [tf1, tau1, tDelay]= obj.firstOrderApprox();
                    y1 = step(tf1, t);
                end
                
                if p.Results.isPlot
                    if isempty(p.Results.axes)
                        ax = axes;
                    else
                        ax = p.Results.axes;
                    end
                    
                    plot(ax, t, y, 'bo-');
                    if obj.is1stOrderApproxAvailable && p.Results.calcApprox
                        hold(ax, 'on'); 
                        plot(ax, t, y1, 'r.-'); 
                        hold(ax, 'off');
                        legend(ax, {'Model', sprintf('1st Order Approx.\n t_{delay} = %5.2e s\n tau_1 = %5.2e s', tDelay, tau1)}, 'location', 'southeast');                        
                    end                    
                    grid(ax, 'on'); 
                    xlim(ax, minmax(t.')); 
                    ylim(ax, [min(y) 1.1*max(y)]);
                    title(ax, obj.name);
                end
            end
        end
        
        function newLTI = mtimes(obj, LTI)
            tfFunc = obj.tfFunction*LTI.tfFunction;
            num1 = tfFunc.Numerator{1}; den1 = tfFunc.Denominator{1};
            newLTI = EquipmentControl.FeedBackOptimization.LTIUnit.AbstractLTIUnit([obj.name '*' LTI.name], num1, den1);
        end
        
    end
end

function res = padZerosLeft(arr, len)
    res = zeros(1, len);
    res(end-length(arr)+1:end) = arr;
end

